package TrainController;
//import JavaFX.*;

//import java.lang.Math;
import TrainModel.TrainModel;
import TrackModel.Block;
import application.Main;

/* Vital Operations deal with the calculation of power given a set of velocity set points. 
 * Ensures vitality of calculation.
 * 
 * */
public class VitalOperations 
{
	 
	public TrainModel trainModel;
	final double dEMPTY_TRAIN = 40900;//90169.065;
	final double dPMAX = 120000;
	final double dMaxSpeed = 19.4444;//m/s
	private long tickLen;
	private boolean bWheelSlip;
	private double dPassWeight = 0;
	private double integral;
	private int iCTCA;
	private int iAuthFlag;
	private double dMBOA;
	private double dUk;
	private double dPowerCommand;
	private double dMaxAccel = .5; 
	private double dRemainingBlock;
	private Block authBlock;
	private Block currBlock;
	private Block tBlock;
	private String header;

	public VitalOperations(TrainModel tm, int tID)
	{
		trainModel = tm;
		header = new String("Train Controller "+tID+" Vital Operations: ");

	}
	
	

	double calcPower(double mvelo, double currvelo, int passcount, long tL)
	{
		Main.log.write(header+"command velocity: " + mvelo + "mph");
		Main.log.write(header+"current velocity: " + currvelo*2.23694 + "mph");
		if((trainModel.getSpeed()*2.23) > 43)
		{
			mvelo = trainModel.getCurrentBlock().getSpeedLimit()*.621371;
		}
		dPassWeight = trainModel.getNumPassengers()*68.3;
		double m = dEMPTY_TRAIN + dPassWeight;                       //mass
		double Kp = 0.1;                       //proportional constant
		double Ki = 0.01;                        //proportional constant
		double dt = 1;
		double velo = mvelo*.4474;				//casts to m/s for calculation
		//CONTROL LOOP
	    double error = velo - currvelo;                        //update error
	    integral += error*dt;     //integral term 
	    double u = Kp*error+Ki*integral; //action of control
	    //Helps attenuation
	    if(u > dUk || u < 0)
	    	dUk = u;
	    double dCurrentAccel = (u/dUk)*dMaxAccel;
	    currvelo += dCurrentAccel*dt;
	    dPowerCommand = currvelo*dCurrentAccel*m;
	    if(dPMAX < dPowerCommand)
	    	dPowerCommand = dPMAX;
	    //P is proportional to v
	    if(currvelo > velo)
	    	dPowerCommand /= 2;
		
		if(bWheelSlip == true)
		{
			dPowerCommand = dPowerCommand/4; 
			bWheelSlip = false;
		}
		
		if(trainModel.getWheelSlip())
		{
			Main.log.write(header+"WHEELS SLIPPING!!!");
			Main.log.write(header+"Power would have been: " + dPowerCommand + "W");
			dPowerCommand = 0;
			bWheelSlip = true;
		}
		
		
		Main.log.write(header+"Power To Pass: " + dPowerCommand + "W");
		double truerror = 0;
		if(currvelo != 0)
		{
			truerror = currvelo/velo;
		}
		if(truerror > 1.35)
		{
			dPowerCommand = 0;
			this.serviceBrake(.20);
		}
		return dPowerCommand;
	}
	
	void calculateAuthority(Block now)
	{
		double bRate;
		double remainingTrack = 0;
		if(iAuthFlag == 0)
		{
			if(iCTCA == 1 && (trainModel.getSpeed() != 0))
			{
				tBlock = trainModel.getCurrentBlock();//just to get block length
				dRemainingBlock = tBlock.getLength();
				iAuthFlag = 1;
				Main.log.write(header+"Locking Auth flag to gather remaining track in final block");
				double vi = Math.pow(trainModel.getSpeed(),2);
				double ourAccel = vi/(2*dRemainingBlock);
				if(ourAccel > 1.2)
				{
					bRate = 1;
				}
				else
				{
					bRate = ourAccel/1.2;
				}	
				this.serviceBrake(bRate);
				Main.log.write(header+"setting brake to "+bRate*100+"% due to CTCAuth being 1");
				remainingTrack = dRemainingBlock - trainModel.getSpeed()*(tickLen/1000);
			}
		}
		if(iAuthFlag == 1 && (trainModel.getSpeed() != 0))
		{
			double pvi = Math.pow(trainModel.getSpeed(),2);
			double pourAccel = pvi/(2*remainingTrack);
			if(pourAccel>1.2)
			{
			
				bRate = 1;
			}
			else
			{
				bRate = pourAccel/1.2;
			}
			this.serviceBrake(bRate);
			Main.log.write(header+"setting brake to "+bRate*100+"% due to CTCAuth being 1");
			remainingTrack = remainingTrack - trainModel.getSpeed()*(tickLen/1000); //milliseconds
		}
		
		
		if(now != currBlock && iCTCA > 1)//so authority isn't decremented when it's 1
		{
			iCTCA--;
			currBlock = now;
			Main.log.write(header+"Decrementing authority to: " +iCTCA);
		}
		
	}
	

	
	
	
	void serviceBrake(double brakerate)//need to utilize in slowing down, but first need safe info from blocks
	{
		trainModel.setBrake(brakerate*100);
		Main.log.write(header+" in serviceBrake: setting brake to "+brakerate*100+"%");
	}
	
	
	void cancelPower()
	{
		dPowerCommand = 0;
	}
	
	void throwEBrake()//must consider if the train has yet to stop moving, potentially need a hazard flag to ensure it fully brakes
	{
			trainModel.setEBrake();
			Main.log.write(header+"throwing the eBrake");
	}
	void updateAuthority(boolean b, int i, double d, Block aBlock)//true we set CTC auth, false we set MBO auth
	{
		if(b)
		{
			iCTCA = i;
			authBlock = aBlock;
			currBlock = authBlock;
			iAuthFlag = 0;
			Main.log.write(header+"updated CTCAuthority to be "+ iCTCA +" blocks");
		}
		else
		{
			dMBOA = d;
			Main.log.write(header+"updated MBOAuthority to be "+ dMBOA +" feet");
		}
	}
	int getCTCAuthority()
	{
		return iCTCA;
	}
}
